//Tarun Nimmagadda
//The University of Texas, Austin

//This file defines the RNDF structure as specified by the DARPA
//specification.  It parses the RNDF file into the RNDF data structure
//#pragma once

#include "RNDF.h"

template <class T>
void change_state(T& previous_state, T& current_state, const T new_state)
    {
    previous_state = current_state;
    current_state = new_state;
    };


RNDF::RNDF(std::string rndfname, bool verbose)
    {
    number_of_segments = number_of_zones = -1;
    is_valid=true;

    std::ifstream rndf_file;

    rndf_file.open(rndfname.c_str());

    if (!rndf_file)
        {
        cout<<"Error: Open file "<<rndfname<<endl;
        is_valid = false;
        return;
        }

    //Parser State
    line_number = 0;

    RNDF_PARSE_STATE state = UNKNOWN, previous_state = UNKNOWN;

    Segment temp_segment;
    Lane temp_lane;
    //Clear temp variables
    temp_segment.clear();
    temp_lane.clear();



    std::string lineread;
    // The main Loop through the whole RNDF file.

    while(getline(rndf_file, lineread)) // Read line by line
        {
        line_number++;
        

        //Blank lines
        if (lineread.size() == 0) 
            {
            if (verbose)
                printf("%d: Blank Line\n",line_number);
            continue;
            }

        std::string token;
        char token_char [DEFAULT_ARRAY_SIZE];
        char temp_char [DEFAULT_ARRAY_SIZE];

        //Read in one line
        sscanf(lineread.c_str(), "%s", token_char);

        token.assign(token_char);


        if (state != COMMENT)
            {
            if (token.compare("RNDF_name") == 0)
                change_state(previous_state, state, GENERAL);
            else if (token.compare("segment") == 0)
                change_state(previous_state, state, SEGMENTS);
            else if (token.compare("lane") == 0)
                change_state(previous_state, state, LANES);
            else if (token.compare("zone") == 0)
                change_state(previous_state, state, ZONES);
            else if (token.compare("perimeter") == 0)
                change_state(previous_state, state, PERIMETER);
            else if (token.compare("spot") == 0)
                change_state(previous_state, state, PARKING_SPOT);
            else if (token.find("/*") != std::string::npos)
                change_state(previous_state, state, COMMENT);
            }

        bool valid=true;

        switch (state)
            {
            case COMMENT:
                if (verbose)
                    printf("%d: COMMENT: %s\n", line_number, lineread.c_str());
                if (lineread.find("*/") != std::string::npos)
                    state = previous_state;
                break;

            case GENERAL:
                //RNDF_NAME
                if (token.compare("RNDF_name") == 0)
                    filename =
                    parse_string(lineread, std::string("RNDF_name"),
                    line_number, valid, verbose);
                //NUM_SEGMENTS
                else if (token.compare("num_segments") == 0)
                    {
                    number_of_segments =
                        parse_integer(lineread, std::string("num_segments"),
                        line_number, valid, verbose);
                    if (number_of_segments <= 0) valid = false;
                    }
                //NUM_ZONES
                else if (token.compare("num_zones") == 0)
                    {
                    number_of_zones =
                        parse_integer(lineread, std::string("num_zones"),
                        line_number, valid, verbose);
                    if (number_of_zones < 0) valid = false;
                    }
                //FORMAT_VERSION
                else if (token.compare("format_version") == 0)
                    format_version =
                    parse_string(lineread, std::string("format_version"),
                    line_number, valid, verbose);
                //CREATION_DATE
                else if (token.compare("creation_date") == 0)
                    creation_date =
                    parse_string(lineread, std::string("creation_date"),
                    line_number, valid, verbose);
                //END_FILE
                else if (token.compare("end_file") == 0)
                    {
                    if (!isvalid())
                        valid = false;
                    else if (verbose)
                        printf("%d: RNDF file has finished parsing\n", line_number);
                    }
                //printf("Token: |%s|\n", token.c_str());
                else {
                    printf("%d: Unexpected token\n", line_number);
                    valid=false;
                    }
                break;

            case SEGMENTS:
                //SEGMENT
                if(token.compare("segment") == 0)
                    {
                    temp_segment.segment_id =
                        parse_integer(lineread, std::string("segment"),
                        line_number, valid, verbose);
                    if (temp_segment.segment_id <= 0) valid = false;
                    }
                //NUM_LANES
                else if(token.compare("num_lanes") == 0){
                    temp_segment.number_of_lanes =
                        parse_integer(lineread, std::string("num_lanes"),
                        line_number, valid, verbose);
                    if (temp_segment.number_of_lanes <= 0) valid = false;
                    }
                //SEGMENT_NAME
                else if(token.compare("segment_name") == 0)
                    temp_segment.segment_name =
                    parse_string(lineread, std::string("segment_name"),
                    line_number, valid, verbose);
                //END_SEGMENT
                else if(token.compare("end_segment") == 0)
                    {
                    if (!temp_segment.isvalid())
                        valid = false;
                    else{
                        segments.push_back(temp_segment);

                        if (verbose)
                            printf("%d: segment has ended\n", line_number);

                        change_state(previous_state, state, GENERAL);

                        temp_segment.clear();
                        }
                    }
                else{
                    printf("%d: Unexpected token\n", line_number);
                    valid=false;
                    }
                break;

            case LANES:
                //LANE
                if(token.compare("lane") == 0)
                    {
                    sprintf(temp_char, "lane %d.%%d", temp_segment.segment_id);

                    if (sscanf(lineread.c_str(), temp_char, &temp_lane.lane_id) == 1)
                        {
                        if (verbose)
                            printf("%d: Lane number is %d\n",
                            line_number, temp_lane.lane_id);
                        }
                    else 
                        valid=false;
                    if (temp_lane.lane_id <= 0) valid = false;
                    }
                //NUM_WAYPOINTS
                else if(token.compare("num_waypoints") == 0)
                    {
                    temp_lane.number_of_waypoints =
                        parse_integer(lineread, std::string("num_waypoints"),
                        line_number, valid, verbose);
                    if (temp_lane.number_of_waypoints <= 0) valid = false;
                    }
                //LANE_WIDTH
                else if(token.compare("lane_width") == 0){
                    temp_lane.lane_width =
                        parse_integer(lineread, std::string("lane_width"),
                        line_number, valid, verbose);
                    if (temp_lane.lane_width <= 0) valid = false;
                    }
                // Signal: ADDED by Jun Ding------------05122011
                else if(token.compare("signal")==0)
                    {
                    //---Important: The pt id must be "1".
                    ElementID e1=ElementID(temp_segment.segment_id,temp_lane.lane_id,1);
                    int Phase_No;

                    sscanf(lineread.c_str(),"signal %*d.%*d.%*d %d",&Phase_No);

                    phaseNo_map[e1]=Phase_No;


                    }
                //LEFT_BOUNDARY
                else if(token.compare("left_boundary") == 0)
                    {
                        printf ("%d: Left Boundary.\n",line_number);
                    }
                //RIGHT_BOUNDARY
                else if(token.compare("right_boundary") == 0)
                    {
                         printf ("%d: Right Boundary.\n",line_number);
                    }
                //CHECKPOINT
                else if(token.compare("checkpoint") == 0)
                    {
                        printf ("%d: checkpoint.\n",line_number);
                    }
                //STOP
                else if(token.compare("stop") == 0)
                    {
                        printf ("%d: STOP.\n",line_number);
                    }
                //EXIT
                else if(token.compare("exit") == 0)
                    {
                    Exit exit(lineread, temp_segment.segment_id,
                        temp_lane.lane_id, line_number, valid, verbose);

                    temp_lane.exits.push_back(exit);
                    //TODO:
                    }
                //END_LANE
                else if(token.compare("end_lane") == 0)
                    {
                    if(temp_lane.number_of_waypoints != (int)temp_lane.waypoints.size())
                        printf("Number of waypoints in lane does not match num_waypoints\n");
                    if (!temp_lane.isvalid())
                        valid = false;
                    else{
                        temp_segment.lanes.push_back(temp_lane);

                        if (verbose)
                            printf("%d: lane has ended\n", line_number);

                        change_state(previous_state, state, SEGMENTS);

                        temp_lane.clear();
                        }
                    }

                //NO TOKEN
                else if(token[0]>=48&&token[0]<=57) // Waypoint, first is a number[0-9]
                    {
                    //WAYPOINT: X.X.X
                    sprintf(temp_char, "%d.%d.", temp_segment.segment_id,
                        temp_lane.lane_id);

                    if(token.find(temp_char) != std::string::npos )
                        {
                        LL_Waypoint wp(lineread, temp_segment.segment_id,
                            temp_lane.lane_id, line_number, valid, verbose);
                        temp_lane.waypoints.push_back(wp);
                        }
                    else{
                        printf("%d: Unexpected token\n", line_number);
                        valid=false;
                        }
                    }
                else 
                    {
                    // num_left or num_right in our case?????
                    printf ("%d: DO NOT Handle now!\n",line_number);
                    }
                break;

            case ZONES:                
            case PERIMETER:                
            case PARKING_SPOT:
                
                break;

            case UNKNOWN:
                printf("%d: Unexpected token\n", line_number);
                valid=false;
            } //------- end of switch(state)

        if (!valid) 
            {
            is_valid=false;
            print_error_message(line_number, token);
            return;
            }

        } //--------  end of While (getline)

    prep_graph();

    if (verbose) printf("Parser Finishes\n");
    }



LL_Waypoint::LL_Waypoint(std::string line, int x, int y,
                         int line_number, bool& valid, bool verbose) 
    {

     char temp_char [DEFAULT_ARRAY_SIZE];
     sprintf(temp_char, "%d.%d.%%d %%lf %%lf", x, y);
     if(sscanf(line.c_str(), temp_char, &waypoint_id, &ll.latitude, &ll.longitude) == 3 
         && isvalid())
         {
         if (verbose)
             {
             printf("%d: ", line_number);
             print();
             }
         }
     else{
         //    clear();
         valid=false;
         }
    };


std::string parse_string(std::string line, std::string token,
                         int line_number, bool& valid, bool verbose)
    {
         char temp_char [DEFAULT_ARRAY_SIZE];
         temp_char[0]='\0';
         if (sscanf(line.c_str(), "%*s %s", temp_char)){
             if (verbose)
                 printf("%d: %s is %s\n", line_number, token.c_str(), temp_char);
             }
         else
             valid=false;

         return std::string(temp_char);
    };

//WITH STRING TOKEN
int parse_integer(std::string line, std::string token,
                  int line_number, bool& valid, bool verbose)
    {
      int integer = INT_MIN;
      if (sscanf(line.c_str(), "%*s %d", &integer) == 1)
          {
          if (verbose)
              printf("%d: %s is %d\n", line_number, token.c_str(), integer);
          }
      else
          valid=false;

      if (integer == INT_MIN) valid = false;
      return integer;
    };

//WITHOUT TOKEN
int parse_integer(std::string line, int line_number, bool& valid,
                  bool verbose)
    {
      int integer = INT_MIN;
      if (sscanf(line.c_str(), "%d", &integer) == 1)
          {
          if (verbose)
              printf("%d: %d\n", line_number, integer);
          }
      else
          valid=false;
      if (integer == INT_MIN) valid = false;
      return integer;
    };

void Lane::clear()
    {
    waypoints.clear();
    left_boundary = right_boundary = UNDEFINED;
    lane_id = number_of_waypoints = INT_MIN;
    lane_width = 0;
    };

void Segment::clear()
    {
    lanes.clear();
    segment_name = std::string("default");
    segment_id = number_of_lanes = INT_MIN;
    };


void RNDF::print()
    {

    if (!is_valid) 
        {
        printf("RNDF not valid\n");
        return;
        }

    printf("RNDF name is %s\n", filename.c_str());
    printf("Number of segments is %d\n", number_of_segments);
    printf("format version is %s\n", format_version.c_str());
    printf("creation date is %s\n", creation_date.c_str());
    print_vector(segments);
    };

Exit::Exit(std::string line, int x, int y, int line_number, bool& valid,
           bool verbose) {

               char temp_char [DEFAULT_ARRAY_SIZE];
               sprintf(temp_char,"exit %d.%d.%%d %%d.%%d.%%d", x, y);
               start_point.segment_id = x;
               start_point.lane_id = y;
               if (sscanf(line.c_str(), temp_char, &start_point.waypoint_id,
                   &end_point.segment_id, &end_point.lane_id,
                   &end_point.waypoint_id) == 4  && isvalid()){
                       if (verbose){
                           printf("%d: ", line_number);
                           print();
                           }
                   }
               else{
                   //    clear();
                   //    start_point.segment_id = x;
                   //    start_point.lane_id = y;
                   valid=false;
                   }
    };

void Segment::print()
    {
    printf("Segment number is %d\n", segment_id);
    printf("Number of Lanes in Segment %d\n", number_of_lanes);
    printf("segment name is %s\n", segment_name.c_str());
    print_vector(lanes);
    };

void Lane::print()
    {
    printf("Lane number is %d\n", lane_id);
    printf("Number of Waypoints in lane %d\n", number_of_waypoints);
    printf("Width of lane %d\n", lane_width);
    print_vector(waypoints);
    };

void Exit::print(){

    printf("Exit from ");
    start_point.print();

    printf(" to ");
    end_point.print();

    printf("\n");
    };


template <class T>
void print_vector (std::vector<T> vec) 
    {
    typename std::vector<T>::iterator i;
    for(i = vec.begin(); i != vec.end(); i++)
        i->print();
    };


void RNDF::prep_graph()
    {

    int index = 0;
    std::vector<Segment>::iterator si;
    std::vector<Lane>::iterator li;
    std::vector<LL_Waypoint>::iterator wi;
    std::vector<WayPointNode>::iterator wpni;
    std::vector<Exit>::iterator exit_i;

    //-----VERTICES-------
    for(si = segments.begin(); si != segments.end(); si++)
        {
        for(li = si->lanes.begin(); li != si->lanes.end(); li++)
            {
            // TO map the waypoint
            for(wi = li->waypoints.begin(); wi != li->waypoints.end(); wi++)
                {
                WayPointNode wpn;
                wpn.ll = wi->ll;
                if (li->lane_width == 0)
                    wpn.lane_width = DEFAULT_LANE_WIDTH;
                else 
                    wpn.lane_width = (li->lane_width);

                wpn.id = ElementID(si->segment_id, li->lane_id, wi->waypoint_id);
                wpn.index = index;
                index++;
                id_map[wpn.id] = (wpn);
                }
            // TO map the exit position.
            for(exit_i=li->exits.begin();exit_i!=li->exits.end();exit_i++)
                {
                ElementID e1=ElementID(si->segment_id, li->lane_id,exit_i->start_point.waypoint_id);
                //LatLong ll_temp=id_map[e1].ll;

                exit_map[e1]=id_map[e1].ll;

                }


            }
        }


 
    }



void print_error_message(int line_number, std::string token)
    {
    printf("%d: Invalid %s\n", line_number, token.c_str());
    }
